#include <Arduino.h>

// void FBM320_GetCoeff(void);
// void FBM320_Calculate(int32_t UP, int32_t UT);

typedef struct FBMTYPE {
    int32_t ADPress;
    int32_t ADTemp;
    float RP;
    float RT;
    float RPFilter;
    float InitPress;
    float Altitude;
    float AltitudeFilter;
    uint16_t C0, C1, C2, C3, C6, C8, C9, C10, C11, C12;
    uint32_t C4, C5, C7;

} FBMTYPE;
FBMTYPE FBM;

#define SCL 26
#define SDA 21
#define SDA_H digitalWrite(SDA, HIGH)
#define SCL_H digitalWrite(SCL, HIGH)
#define SDA_L digitalWrite(SDA, LOW)
#define SCL_L digitalWrite(SCL, LOW)
#define SDA_read digitalRead(SDA)

#define FBMAddr 0xD8
#define SPI_CTRL 0x00
#define FBM_ID 0x6B
#define FBM_COEFF1 0xAA
#define FBM_COEFF2 0xBB
#define FBM_COEFF3 0xD0
#define FBM_COEFF4 0xF1
#define FBM_RESET 0xE0
#define FBM_CONFIG 0xF4
#define DATA_MSB 0xF6
#define DATA_CSB 0xF7
#define DATA_LSB 0xF8
#define OSR1024 0x00
#define OSR2048 0x40
#define OSR4096 0x80
#define OSR8192 0xC0
#define PRES_CONVERSION 0x34
#define TEMP_CONVERSION 0x2E
#define FBMRESET 0xB6
#define FBMID 0x42

void setup() {
    // put your setup code here, to run once:
    Serial.begin(115200);
    pinMode(SDA, OUTPUT);
    pinMode(SCL, OUTPUT);
    digitalWrite(SDA, HIGH);
    digitalWrite(SCL, HIGH);
    Serial.println("Program Started!");

    FBM320_Init();
}

void loop() {
    // put your main code here, to run repeatedly:
    FBM320_GetAltitude();
}

void SDA_OUT(void) { pinMode(SDA, OUTPUT); }

void SDA_IN(void) { pinMode(SDA, INPUT); }

void Delay_us(int n) { delayMicroseconds(n); }

void IIC_Start(void) {
    SDA_OUT();
    SDA_H;
    SCL_H;
    Delay_us(4);
    SDA_L;
    Delay_us(4);
    SCL_L;
}

void IIC_Stop(void) {
    SDA_OUT();
    SCL_L;
    SDA_L;
    Delay_us(4);
    SCL_H;
    SDA_H;
    Delay_us(4);
}

uint8_t IIC_WaitAck(void) {
    uint8_t ucErrTime = 0;
    SDA_IN();  // SDAÉèÖÃÎªÊäÈë  £¨´Ó»ú¸øÒ»¸öµÍµçÆ½×öÎªÓ¦´ð£©
    SDA_H;
    Delay_us(1);
    SCL_H;
    Delay_us(1);
    while (SDA_read) {
        ucErrTime++;
        if (ucErrTime > 250) {
            IIC_Stop();
            return 1;
        }
    }
    SCL_L;  //Ê±ÖÓÊä³öµÍ
    return 0;
}

void IIC_Ack(void) {
    SCL_L;
    SDA_OUT();
    SDA_L;
    Delay_us(1);
    SCL_H;
    Delay_us(2);
    SCL_L;
}

void IIC_NAck(void) {
    SCL_L;
    SDA_OUT();
    SDA_H;
    Delay_us(1);
    SCL_H;
    Delay_us(1);
    SCL_L;
}

void IIC_SendByte(uint8_t data) {
    uint8_t t;
    SDA_OUT();
    SCL_L;  //À­µÍÊ±ÖÓ¿ªÊ¼Êý¾Ý´«Êä
    for (t = 0; t < 8; t++) {
        if (data & 0x80)
            SDA_H;
        else
            SDA_L;
        data <<= 1;
        Delay_us(1);
        SCL_H;
        Delay_us(1);
        SCL_L;
        Delay_us(1);
    }
}

uint8_t IIC_ReadByte(uint8_t ack) {
    uint8_t i, receive = 0;
    SDA_IN();  // SDAÉèÖÃÎªÊäÈëÄ£Ê½ µÈ´ý½ÓÊÕ´Ó»ú·µ»ØÊý¾Ý
    for (i = 0; i < 8; i++) {
        SCL_L;
        Delay_us(1);
        SCL_H;
        receive <<= 1;
        if (SDA_read) receive++;  //´Ó»ú·¢ËÍµÄµçÆ½
        Delay_us(1);
    }
    if (ack)
        IIC_Ack();  //·¢ËÍACK
    else
        IIC_NAck();  //·¢ËÍnACK
    return receive;
}

uint8_t IIC_ReadByteFromSlave(uint8_t I2C_Addr, uint8_t reg, uint8_t *buf) {
    IIC_Start();
    IIC_SendByte(I2C_Addr);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck())       //Èç¹û´Ó»úÎ´Ó¦´ðÔòÊý¾Ý·¢ËÍÊ§°Ü
    {
        IIC_Stop();
        return 1;
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();

    IIC_Start();
    IIC_SendByte(I2C_Addr + 1);  //½øÈë½ÓÊÕÄ£Ê½
    IIC_WaitAck();
    *buf = IIC_ReadByte(0);
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_WriteByteToSlave(uint8_t I2C_Addr, uint8_t reg, uint8_t data) {
    IIC_Start();
    IIC_SendByte(I2C_Addr);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    IIC_SendByte(data);
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //Êý¾ÝÐ´ÈëÊ§°Ü
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_ReadMultByteFromSlave(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data) {
    uint8_t count = 0;
    uint8_t temp;
    IIC_Start();
    IIC_SendByte(dev);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    IIC_Start();
    IIC_SendByte(dev + 1);  //½øÈë½ÓÊÕÄ£Ê½
    IIC_WaitAck();
    for (count = 0; count < length; count++) {
        if (count != (length - 1))
            temp = IIC_ReadByte(1);  //´øACKµÄ¶ÁÊý¾Ý
        else
            temp = IIC_ReadByte(0);  //×îºóÒ»¸ö×Ö½ÚNACK

        data[count] = temp;
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_WriteMultByteToSlave(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data) {
    uint8_t count = 0;
    IIC_Start();
    IIC_SendByte(dev);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    for (count = 0; count < length; count++) {
        IIC_SendByte(data[count]);
        if (IIC_WaitAck())  //Ã¿Ò»¸ö×Ö½Ú¶¼ÒªµÈ´Ó»úÓ¦´ð
        {
            IIC_Stop();
            return 1;  //Êý¾ÝÐ´ÈëÊ§°Ü
        }
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t FBM320_WriteByte(uint8_t reg, uint8_t data) {
    if (IIC_WriteByteToSlave(FBMAddr, reg, data))
        return 1;
    else
        return 0;
}

uint8_t FBM320_ReadByte(uint8_t reg, uint8_t *buf) {
    if (IIC_ReadByteFromSlave(FBMAddr, reg, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_WriteMultBytes(uint8_t reg, uint8_t len, uint8_t *buf) {
    if (IIC_WriteMultByteToSlave(FBMAddr, reg, len, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_ReadMultBytes(uint8_t reg, uint8_t len, uint8_t *buf) {
    if (IIC_ReadMultByteFromSlave(FBMAddr, reg, len, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_getDeviceID(void) {
    uint8_t buf;
    FBM320_ReadByte(FBM_ID, &buf);
    return buf;
}

uint8_t FBM320_testConnection(void) {
    if (FBM320_getDeviceID() == FBMID)  // 0x42
        return 1;
    else
        return 0;
}

void FBM320_Check(void) {
    while (!FBM320_testConnection()) {
        Serial.printf("\rFBM320 no connect...\r\n");
    }
    Serial.printf("Connected\r\n");
}

void FBM320_Init(void) {
    FBM320_Check();  //¼ì²éFBM320ÓëMCUÊÇ·ñÕý³£Í¨ÐÅ

    FBM320_WriteByte(FBM_RESET, FBMRESET);  //¸´Î»FBM320
    delay(100);
    FBM320_GetCoeff();
}

uint8_t Init_Altitude(void) {
    static uint8_t cnt_p = 0;
    static int64_t PressNUM = 0;

    for (int i = 0; i < 100; i++) {
        PressNUM += FBM.RP;
    }
    FBM.InitPress = (float)PressNUM / 100;
    return 0;
}

/******************************************************************************
 * 函  数：void FBM320_GetAltitude(void)
 * 功  能：FBM320的气压值转换成相对高度
 * 参  数：无
 * 返回值：无
 * 备  注：注意：此函数调用频率最好200Hz,否则获取的高度值误差较大！！！
 *******************************************************************************/
void FBM320_GetAltitude(void)  // 200Hz
{
    uint8_t buf[3];
    FBM320_WriteByte(FBM_CONFIG, TEMP_CONVERSION);  // 280us
    delay(5);

    FBM320_ReadMultBytes(DATA_MSB, 3, buf);
    FBM.ADTemp = (buf[0] << 16) | (buf[1] << 8) | buf[2];
    Delay_us(500);
    FBM320_WriteByte(FBM_CONFIG, PRES_CONVERSION + OSR8192);  //开启气压转化
    delay(10);

    FBM320_ReadMultBytes(DATA_MSB, 3, buf);
    FBM.ADPress = (buf[0] << 16) | (buf[1] << 8) | buf[2];
    FBM320_WriteByte(FBM_CONFIG, TEMP_CONVERSION);  //开启温度转换
    delay(3);
    FBM320_Calculate(FBM.ADPress,
                     FBM.ADTemp);  //将FBM320的原始数据转换成物理量 气压单位是帕，温度单位摄氏度
    FBM.Altitude = 44330 * (1 - pow(((double)(FBM.RP / 1000.0)) / ((double)(101.325)), 1.0 / 5.255));
    Serial.printf("Altitude:%0.2f \r\n", FBM.Altitude);
}

void FBM320_GetCoeff(void) {
    uint8_t data[20], i = 0;
    uint16_t R[10];
    FBM320_ReadMultBytes(FBM_COEFF1, 18, data);
    FBM320_ReadByte(FBM_COEFF3, &data[18]);
    FBM320_ReadByte(FBM_COEFF4, &data[19]);
    for (i = 0; i < 10; i++) {
        R[i] = (uint16_t)(data[2 * i] << 8) | data[2 * i + 1];
    }
    /* Use R0~R9 calculate C0~C12 of FBM320-02	*/
    FBM.C0 = R[0] >> 4;
    FBM.C1 = ((R[1] & 0xFF00) >> 5) | (R[2] & 7);
    FBM.C2 = ((R[1] & 0xFF) << 1) | (R[4] & 1);
    FBM.C3 = R[2] >> 3;
    FBM.C4 = ((uint32_t)R[3] << 2) | (R[0] & 3);
    FBM.C5 = R[4] >> 1;
    FBM.C6 = R[5] >> 3;
    FBM.C7 = ((uint32_t)R[6] << 3) | (R[5] & 7);
    FBM.C8 = R[7] >> 3;
    FBM.C9 = R[8] >> 2;
    FBM.C10 = ((R[9] & 0xFF00) >> 6) | (R[8] & 3);
    FBM.C11 = R[9] & 0xFF;
    FBM.C12 = ((R[0] & 0x0C) << 1) | (R[7] & 7);

    for (i = 0; i < 10; i++) Serial.printf("R = %d\n", R[i]);
    Serial.printf("C0=%d C1=%d C2=%d\n", FBM.C0, FBM.C1, FBM.C2);
    Serial.printf("C3=%d C4=%d C5=%d\n", FBM.C3, FBM.C4, FBM.C5);
    Serial.printf("C6=%d C7=%d C8=%d\n", FBM.C6, FBM.C7, FBM.C8);
    Serial.printf("C9=%d C10=%d C11=%d\n", FBM.C9, FBM.C10, FBM.C11);
    Serial.printf("C12=%d\n", FBM.C12);
}

void FBM320_Calculate(int32_t UP, int32_t UT) {
    int32_t DT, DT2, X01, X02, X03, X11, X12, X13, X21, X22, X23, X24, X25, X26, X31, X32, CF, PP1, PP2, PP3,
        PP4;

    DT = ((UT - 8388608) >> 4) + (FBM.C0 << 4);
    X01 = (FBM.C1 + 4459) * DT >> 1;
    X02 = ((((FBM.C2 - 256) * DT) >> 14) * DT) >> 4;
    X03 = (((((FBM.C3 * DT) >> 18) * DT) >> 18) * DT);
    FBM.RT = ((2500 << 15) - X01 - X02 - X03) >> 15;

    DT2 = (X01 + X02 + X03) >> 12;

    X11 = ((FBM.C5 - 4443) * DT2);
    X12 = (((FBM.C6 * DT2) >> 16) * DT2) >> 2;
    X13 = ((X11 + X12) >> 10) + ((FBM.C4 + 120586) << 4);

    X21 = ((FBM.C8 + 7180) * DT2) >> 10;
    X22 = (((FBM.C9 * DT2) >> 17) * DT2) >> 12;
    X23 = (X22 >= X21) ? (X22 - X21) : (X21 - X22);

    X24 = (X23 >> 11) * (FBM.C7 + 166426);
    X25 = ((X23 & 0x7FF) * (FBM.C7 + 166426)) >> 11;
    X26 =
        (X21 >= X22) ? (((0 - X24 - X25) >> 11) + FBM.C7 + 166426) : (((X24 + X25) >> 11) + FBM.C7 + 166426);

    PP1 = ((UP - 8388608) - X13) >> 3;
    PP2 = (X26 >> 11) * PP1;
    PP3 = ((X26 & 0x7FF) * PP1) >> 11;
    PP4 = (PP2 + PP3) >> 10;

    CF = (2097152 + FBM.C12 * DT2) >> 3;
    X31 = (((CF * FBM.C10) >> 17) * PP4) >> 2;
    X32 = (((((CF * FBM.C11) >> 15) * PP4) >> 18) * PP4);
    FBM.RP = ((X31 + X32) >> 15) + PP4 + 99880;  // 99880ÆøÑ¹²¹³¥
}
